/**
* This file is part of LIO-mapping.
* 
* Copyright (C) 2019 Haoyang Ye <hy.ye at connect dot ust dot hk>,
* Robotics and Multiperception Lab (RAM-LAB <https://ram-lab.com>),
* The Hong Kong University of Science and Technology
* 
* For more information please see <https://ram-lab.com/file/hyye/lio-mapping>
* or <https://sites.google.com/view/lio-mapping>.
* If you use this code, please cite the respective publications as
* listed on the above websites.
* 
* LIO-mapping is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
* 
* LIO-mapping is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
* 
* You should have received a copy of the GNU General Public License
* along with LIO-mapping.  If not, see <http://www.gnu.org/licenses/>.
*/

//
// Created by hyye on 3/27/18.
//

#include <ceres/ceres.h>
#include "imu_processor/Estimator.h"

#include <pcl/common/transforms.h>
#include <pcl/search/impl/flann_search.hpp>
#include <pcl/filters/extract_indices.h>

namespace lio {

#define CHECK_JACOBIAN 0

// WARNING to half actually
size_t TransformToEnd(PointCloudPtr &cloud, Twist<float> transform_es, float time_factor, bool keep_intensity = false) {
  size_t cloud_size = cloud->points.size();

  for (size_t i = 0; i < cloud_size; i++) {
    PointT &point = cloud->points[i];

    float s = time_factor * (point.intensity - int(point.intensity));

//    DLOG(INFO) << "s: " << s;
    if (s < 0 || s > 1 + 1e-3) {
      LOG(ERROR) << "point.intensity: " << point.intensity;
      LOG(ERROR) << "time ratio error: " << s;
    }

    point.x -= s * transform_es.pos.x();
    point.y -= s * transform_es.pos.y();
    point.z -= s * transform_es.pos.z();
    if (!keep_intensity) {
      point.intensity -= int(point.intensity);
    }

    Eigen::Quaternionf q_id, q_s, q_e, q_half;
    q_e = transform_es.rot;
    q_id.setIdentity();
    q_s = q_id.slerp(s, q_e);

    RotatePoint(q_s.conjugate().normalized(), point);

//    q_half = q_id.slerp(0.5, q_e);
//    RotatePoint(q_half, point);
//    point.x += 0.5 * transform_es.pos.x();
//    point.y += 0.5 * transform_es.pos.y();
//    point.z += 0.5 * transform_es.pos.z();
     RotatePoint(q_e, point);

     point.x += transform_es.pos.x();
     point.y += transform_es.pos.y();
     point.z += transform_es.pos.z();
  }

  return cloud_size;
}

void averageTimeDownSmaple(PointCloudPtr input, PointCloudPtr spoint_downsampled_, int step = 3) {
  int n = input->points.size();
  for (size_t idx = 0; idx < n; idx+= step) {
    spoint_downsampled_->points.push_back(input->points[idx]);
  }
}

Estimator::Estimator() {
  ROS_DEBUG(">>>>>>> Estimator started! <<<<<<<");

  ClearState();
}

Estimator::Estimator(EstimatorConfig config, MeasurementManagerConfig mm_config) {
  ROS_DEBUG(">>>>>>> Estimator started! <<<<<<<");

  SetupAllEstimatorConfig(config, mm_config);

  ClearState();
}

Estimator::~Estimator() {

}

void Estimator::SetupAllEstimatorConfig(const EstimatorConfig &config, const MeasurementManagerConfig &mm_config) {

  this->mm_config_ = mm_config;

  if (estimator_config_.window_size != config.window_size) {
    linear_acceleration_buf_.Reset(config.window_size + 1);
    angular_velocity_buf_.Reset(config.window_size + 1);
    imu_timestamp_buf_.Reset(config.window_size + 1);
    size_surf_stack_.Reset(config.window_size + 1);
    size_corner_stack_.Reset(config.window_size + 1);
  }

  down_size_filter_corner_.setLeafSize(config.corner_filter_size, config.corner_filter_size, config.corner_filter_size);
  down_size_filter_surf_.setLeafSize(config.surf_filter_size, config.surf_filter_size, config.surf_filter_size);
  down_size_filter_map_.setLeafSize(config.map_filter_size, config.map_filter_size, config.map_filter_size);
  transform_lb_ = config.transform_lb;

  min_match_sq_dis_ = config.min_match_sq_dis;
  min_plane_dis_ = config.min_plane_dis;
  extrinsic_stage_ = config.estimate_extrinsic;

  estimator_config_ = config;
}

void Estimator::ClearState() {
  // TODO: CirclarBuffer should have clear method

  for (size_t i = 0; i < estimator_config_.window_size + 1;
       ++i) {
    linear_acceleration_buf_[i].clear();
    angular_velocity_buf_[i].clear();
    size_surf_stack_[i] = 0;
    size_corner_stack_[i] = 0;

    imu_timestamp_buf_[i].clear();
  }

  stage_flag_ = NOT_INITED;
  first_imu_ = false;
  cir_buf_count_ = 0;

  R_WI_.setIdentity();
  Q_WI_ = R_WI_;

  // WARNING: g_norm should be set before clear
  g_norm_ = 9.805;

  convergence_flag_ = false;
}

void Estimator::SetupRos(ros::NodeHandle &nh) {
  MeasurementManager::SetupRos(nh);
  PointMapping::SetupRos(nh, false);

  wi_trans_.frame_id_ = "camera_init";
  wi_trans_.child_frame_id_ = "world";

  wi_trans_.setRotation(tf::Quaternion(0, 0, 0, 1));
  wi_trans_.setOrigin(tf::Vector3(0, 0, 0));

  laser_local_trans_.frame_id_ = "world";
  laser_local_trans_.child_frame_id_ = "laser_local";

  laser_local_trans_.setRotation(tf::Quaternion(0, 0, 0, 1));
  laser_local_trans_.setOrigin(tf::Vector3(0, 0, 0));

  laser_send_trans_.frame_id_ = "world";
  laser_send_trans_.child_frame_id_ = "laser_send";

  laser_send_trans_.setRotation(tf::Quaternion(0, 0, 0, 1));
  laser_send_trans_.setOrigin(tf::Vector3(0, 0, 0));

  laser_predict_trans_.frame_id_ = "laser_local";
  laser_predict_trans_.child_frame_id_ = "laser_predict";
  laser_predict_trans_.setIdentity();

  predict_odom_.header.frame_id = "world";
  predict_odom_.child_frame_id = "imu_predict";
  pub_predict_odom_ = nh.advertise<nav_msgs::Odometry>("/predict_odom", 100);

  predict_spline_odom_.header.frame_id = "world";
  predict_spline_odom_.child_frame_id = "imu_spline_predict";
  pub_predict_spline_odom_ = nh.advertise<nav_msgs::Odometry>("/predict_spline_odom", 100);

  laser_odom_.header.frame_id = "world";
  laser_odom_.child_frame_id = "laser_predict";
  pub_laser_odom_ = nh.advertise<nav_msgs::Odometry>("/predict_laser_odom", 100);

  local_odom_.header.frame_id = "world";
  local_odom_.child_frame_id = "laser_predict";
  pub_local_odom_ = nh.advertise<nav_msgs::Odometry>("/local_laser_odom", 100);

  pub_plane_normal_ = nh.advertise<visualization_msgs::MarkerArray>("/debug/plane_normal", 5);

  pub_local_surf_points_ = nh.advertise<sensor_msgs::PointCloud2>("/local/surf_points", 2);
  pub_local_corner_points_ = nh.advertise<sensor_msgs::PointCloud2>("/local/corner_points", 2);
  pub_local_full_points_ = nh.advertise<sensor_msgs::PointCloud2>("/local/full_points", 2);

  pub_map_surf_points_ = nh.advertise<sensor_msgs::PointCloud2>("/map/surf_points", 2);
  pub_map_corner_points_ = nh.advertise<sensor_msgs::PointCloud2>("/map/corner_points", 2);
  pub_predict_surf_points_ = nh.advertise<sensor_msgs::PointCloud2>("/predict/surf_points", 2);
  pub_predict_corner_points_ = nh.advertise<sensor_msgs::PointCloud2>("/predict/corner_points", 2);
  pub_predict_full_points_ = nh.advertise<sensor_msgs::PointCloud2>("/predict/full_points", 2);
  pub_predict_corrected_full_points_ = nh.advertise<sensor_msgs::PointCloud2>("/predict/corrected_full_points", 2);

  pub_extrinsic_ = nh.advertise<geometry_msgs::PoseStamped>("/extrinsic_lb", 10);
}

void Estimator::ProcessImu(double dt,
                           const Vector3d &linear_acceleration,
                           const Vector3d &angular_velocity) {
    
    Eigen::Vector3d &bas = traj_manager_->calib_param_manager->acce_bias;
    Eigen::Vector3d &bgs = traj_manager_->calib_param_manager->gyro_bias;
    g_vec_ = traj_manager_->calib_param_manager->gravity;

    Vector3d un_acc_0 = imu_data_last_.q * (imu_data_last_.a - bas) + g_vec_;
    Vector3d un_gyr = 0.5 * (imu_data_last_.w + angular_velocity) - bgs;
    imu_data_last_.q *= DeltaQ(un_gyr * dt);
    Vector3d un_acc_1 = imu_data_last_.q * (linear_acceleration - bas) + g_vec_;
    Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
    imu_data_last_.p += dt * imu_data_last_.v + 0.5 * dt * dt * un_acc;
    imu_data_last_.v += dt * un_acc;

}

// TODO: this function can be simplified
void Estimator::ProcessLaserOdom(const Transform &transform_in, const std_msgs::Header &header) {

  ROS_DEBUG(">>>>>>> new laser odom coming <<<<<<<");

  ++laser_odom_recv_count_;

  if (stage_flag_ != INITED
      && laser_odom_recv_count_ % estimator_config_.init_window_factor != 0) { /// better for initialization
    return;
  }
  // 获取初始时间，直接用时间戳时间太大了，不便于观察
  if (!is_get_time_init_) {
    time_init_ = header.stamp.toSec();
    is_get_time_init_ = true;
  }

  // frame_timestamp_buf_.push(header.stamp.toSec());

  // TODO: avoid memory allocation?
  if (stage_flag_ != INITED || (!estimator_config_.enable_deskew && !estimator_config_.cutoff_deskew)) {
    for (auto &p : laser_cloud_surf_stack_downsampled_->points) {
      p.intensity = 0;
    }
    // surf_stack_.push(boost::make_shared<PointCloud>(*laser_cloud_surf_stack_downsampled_));
    size_surf_stack_.push(laser_cloud_surf_stack_downsampled_->size());

    for (auto &p : laser_cloud_corner_stack_downsampled_->points) {
      p.intensity = 0;
    }
    // corner_stack_.push(boost::make_shared<PointCloud>(*laser_cloud_corner_stack_downsampled_));
    size_corner_stack_.push(laser_cloud_corner_stack_downsampled_->size());
  }

  // full_stack_.push(boost::make_shared<PointCloud>(*full_cloud_));

  if (estimator_config_.run_optimization) {
    switch (stage_flag_) {
      case NOT_INITED: {

        {
          DLOG(INFO) << "size_surf_stack_: " << size_surf_stack_.size();
          DLOG(INFO) << "size_corner_stack_: " << size_corner_stack_.size();
        }

        bool init_result = false;
        if (cir_buf_count_ == estimator_config_.window_size) {
          tic_toc_.Tic();

          if (!estimator_config_.imu_factor) {
            init_result = true;
            // TODO: update states Ps_
          } else {

            if (extrinsic_stage_ == 2) {
              // TODO: move before initialization
              // bool extrinsic_result = ImuInitializer::EstimateExtrinsicRotation(all_laser_transforms_, transform_lb_);
              // LOG(INFO) << ">>>>>>> extrinsic calibration"
              //           << (extrinsic_result ? " successful"
              //                                : " failed")
              //           << " <<<<<<<";
              // if (extrinsic_result) {
              //   extrinsic_stage_ = 1;
              //   DLOG(INFO) << "change extrinsic stage to 1";
              // }
            }

            if (extrinsic_stage_ != 2 && (header.stamp.toSec() - initial_time_) > 0.1) {
              DLOG(INFO) << "EXTRINSIC STAGE: " << extrinsic_stage_;
              // init_result = RunInitialization();
              init_result = true;
              initial_time_ = header.stamp.toSec();
            }
          }

          DLOG(INFO) << "initialization time: " << tic_toc_.Toc() << " ms";

          if (init_result) {
            stage_flag_ = INITED;
            SetInitFlag(true);

            Q_WI_ = R_WI_;
//            wi_trans_.setRotation(tf::Quaternion{Q_WI_.x(), Q_WI_.y(), Q_WI_.z(), Q_WI_.w()});

            ROS_WARN_STREAM(">>>>>>> IMU initialized <<<<<<<");

            if (estimator_config_.enable_deskew || estimator_config_.cutoff_deskew) {
              ros::ServiceClient client = nh_.serviceClient<std_srvs::SetBool>("/enable_odom");
              std_srvs::SetBool srv;
              srv.request.data = 0;
              if (client.call(srv)) {
                DLOG(INFO) << "TURN OFF THE ORIGINAL LASER ODOM";
              } else {
                LOG(FATAL) << "FAILED TO CALL TURNING OFF THE ORIGINAL LASER ODOM";
              }
            }

            // TODO: 初始化
            SplineInitialization();

          } else {
            SlideWindow();
          }
        } else {
        
          SlideWindow();

          ++cir_buf_count_;
        }

        break;
      }
      case INITED: {

        // TODO

        // WARNING

        if (estimator_config_.opt_window_size + 1) {

          // TODO: 此处不需要去畸变
          if (0) {
          if (estimator_config_.enable_deskew || estimator_config_.cutoff_deskew) {
            TicToc t_deskew;
            t_deskew.Tic();
            // TODO: the coefficients to be parameterized
            DLOG(INFO) << ">>>>>>> de-skew points <<<<<<<";
            LOG_ASSERT(imu_stampedtransforms.size() > 0) << "no imu data";
            double time_e = imu_stampedtransforms.last().time;
            Transform transform_e = imu_stampedtransforms.last().transform;
            double time_s = imu_stampedtransforms.last().time;
            Transform transform_s = imu_stampedtransforms.last().transform;
            for (int i = int(imu_stampedtransforms.size()) - 1; i >= 0; --i) {
              time_s = imu_stampedtransforms[i].time;
              transform_s = imu_stampedtransforms[i].transform;
              if (time_e - imu_stampedtransforms[i].time >= 0.1) {
                break;
              }
            }
//            Eigen::Vector3d body_velocity, vel1, vel2;
//            vel1 = Rs_[estimator_config_.window_size - 1].transpose() * Vs_[estimator_config_.window_size - 1];
//            vel2 = Rs_[estimator_config_.window_size].transpose() * Vs_[estimator_config_.window_size];
//            body_velocity = (vel1 + vel2) / 2;

            Transform transform_body_es = transform_e.inverse() * transform_s;
//            transform_body_es.pos = -0.1 * body_velocity.cast<float>();
            {
              float s = 0.1 / (time_e - time_s);
              Eigen::Quaternionf q_id, q_s, q_e, q_half;
              q_e = transform_body_es.rot;
              q_id.setIdentity();
              q_s = q_id.slerp(s, q_e);
              transform_body_es.rot = q_s;
              transform_body_es.pos = s * transform_body_es.pos;
            }

            transform_es_ = transform_lb_ * transform_body_es * transform_lb_.inverse();
            DLOG(INFO) << "time diff: " << time_e - time_s;
            DLOG(INFO) << "transform diff: " << transform_es_;
            DLOG(INFO) << "transform diff norm: " << transform_es_.pos.norm();

            if (!estimator_config_.cutoff_deskew) {
              TransformToEnd(laser_cloud_surf_last_, transform_es_, 10);

              TransformToEnd(laser_cloud_corner_last_, transform_es_, 10);
#ifdef USE_CORNER
              TransformToEnd(corner_stack_.last(), transform_es_, 10);
#endif
            } else {
              DLOG(INFO) << "cutoff_deskew";
            }

            laser_cloud_surf_stack_downsampled_->clear();
            down_size_filter_surf_.setInputCloud(laser_cloud_surf_last_);
            down_size_filter_surf_.filter(*laser_cloud_surf_stack_downsampled_);
            size_t laser_cloud_surf_stack_ds_size = laser_cloud_surf_stack_downsampled_->points.size();

            // down sample feature stack clouds
            laser_cloud_corner_stack_downsampled_->clear();
            down_size_filter_corner_.setInputCloud(laser_cloud_corner_last_);
            down_size_filter_corner_.filter(*laser_cloud_corner_stack_downsampled_);
            size_t laser_cloud_corner_stack_ds_size = laser_cloud_corner_stack_downsampled_->points.size();

            // surf_stack_.push(boost::make_shared<PointCloud>(*laser_cloud_surf_stack_downsampled_));
            size_surf_stack_.push(laser_cloud_surf_stack_downsampled_->size());

            // corner_stack_.push(boost::make_shared<PointCloud>(*laser_cloud_corner_stack_downsampled_));
            size_corner_stack_.push(laser_cloud_corner_stack_downsampled_->size());

            ROS_DEBUG_STREAM("deskew time: " << t_deskew.Toc());

            DLOG(INFO) << "deskew time: " << t_deskew.Toc();
          }
          }

          laser_cloud_surf_stack_downsampled_->clear();
          // down_size_filter_surf_.setInputCloud(laser_cloud_surf_last_);
          // down_size_filter_surf_.filter(*laser_cloud_surf_stack_downsampled_);
          averageTimeDownSmaple(laser_cloud_surf_last_, laser_cloud_surf_stack_downsampled_);
          size_t laser_cloud_surf_stack_ds_size = laser_cloud_surf_stack_downsampled_->points.size();

          // down sample feature stack clouds
          laser_cloud_corner_stack_downsampled_->clear();
          // down_size_filter_corner_.setInputCloud(laser_cloud_corner_last_);
          // down_size_filter_corner_.filter(*laser_cloud_corner_stack_downsampled_);
          averageTimeDownSmaple(laser_cloud_corner_last_, laser_cloud_corner_stack_downsampled_);
          size_t laser_cloud_corner_stack_ds_size = laser_cloud_corner_stack_downsampled_->points.size();

          // surf_stack_.push(boost::make_shared<PointCloud>(*laser_cloud_surf_stack_downsampled_));
          size_surf_stack_.push(laser_cloud_surf_stack_downsampled_->size());

          // corner_stack_.push(boost::make_shared<PointCloud>(*laser_cloud_corner_stack_downsampled_));
          size_corner_stack_.push(laser_cloud_corner_stack_downsampled_->size());

          // TODO: 根据最新的点云及 IMU 时间增加控制点，此时的初值为最后一个控制点的值
          // int knotNum = traj_manager_->getTrajectory()->R3Spline()->NumKnots();
          // // 固定目前已有的最后4个控制点，避免利用imu数据给控制点赋初值时，改变之前已经优化好的控制点
          // traj_manager_->getTrajectory()->R3Spline()->LockContralPoint(knotNum - 4, 4);
          // traj_manager_->getTrajectory()->SO3Spline()->LockContralPoint(knotNum - 4, 4);
          LOG(INFO) << "in normal extendTrajTo";
          // double time_last = frame_timestamp_buf_.last();
          // traj_manager_->extendTrajTo(time_last, time_last);

          // TODO: 为新增的控制点赋初值
          LOG(INFO) << "in normal SetNewContralPoint";
          // SetNewContralPoint();
          // predict_odom_.header.stamp = Headers_.last().stamp;
          // predict_odom_.header.seq += 1;
          // Eigen::Quaterniond quat(Rs_.last());
          // Eigen::Vector3d pos(Ps_.last());
          // predict_odom_.pose.pose.orientation.x = quat.x();
          // predict_odom_.pose.pose.orientation.y = quat.y();
          // predict_odom_.pose.pose.orientation.z = quat.z();
          // predict_odom_.pose.pose.orientation.w = quat.w();
          // predict_odom_.pose.pose.position.x = pos.x();
          // predict_odom_.pose.pose.position.y = pos.y();
          // predict_odom_.pose.pose.position.z = pos.z();

          pub_predict_odom_.publish(predict_odom_);

          // 计算pivot处imu在I0系下的位姿
          Eigen::Vector3d pivot_imu_pos;
          Eigen::Quaterniond pivot_imu_rot;
          int pivot_idx = int(estimator_config_.window_size - estimator_config_.opt_window_size);
          if (!traj_manager_->evaluateImuPose(frame_timestamp_buf_[pivot_idx], pivot_imu_pos, pivot_imu_rot)) {
            LOG(ERROR) << "could not get pivot pose at I0 with t= " << frame_timestamp_buf_[pivot_idx];
          }
          
          // 初始化估计器
          for (int iter = 0; iter < 1; ++iter) {
          LOG(INFO) << "in normal trajInit";
          traj_manager_->trajInit();

          // TODO: 添加测量残差
          vector<FeaturePerFrame> feature_frames;
          LOG(INFO) << "in normal BuildLocalMapSplineNormal";
          tic_toc_.Tic();
          bool first_opt = iter == 0;
          LOG(INFO) << "first_opt type = " << first_opt;
          BuildLocalMapSplineNormal(feature_frames, pivot_imu_pos, pivot_imu_rot, first_opt);
          LOG(INFO) << "BuildLocalMapSplineNormal, t = " << tic_toc_.Toc() << " ms";
          // 根据找到的匹配对，添加残差
          tic_toc_.Tic();

          for (int i = 1; i < estimator_config_.opt_window_size + 1; ++i) {
            int opt_i = int(estimator_config_.window_size - estimator_config_.opt_window_size + i);
            int pivot_idx = int(estimator_config_.window_size - estimator_config_.opt_window_size);

            FeaturePerFrame &feature_per_frame = feature_frames[opt_i];
            LOG_ASSERT(opt_i == feature_per_frame.id);

            vector<unique_ptr<Feature>> &features = feature_per_frame.features;

            DLOG(INFO) << "features.size(): " << features.size();

            for (int j = 0; j < features.size(); ++j) {
              PointPlaneFeature feature_j;
              features[j]->GetFeature(&feature_j);
              // 暂时将s作为相对时间
              const double &s = feature_j.score;
              // LOG(INFO) << "S = " << s;

              Eigen::Vector3d &p_eigen = feature_j.point;
              Eigen::Vector4d &coeff_eigen = feature_j.coeffs;
              // traj_manager_->addPivotPointPlaneMeasurement(p_eigen, coeff_eigen, 
              //                                             frame_timestamp_buf_[pivot_idx],
              //                                             frame_timestamp_buf_[opt_i]);

              // 给出pivot处imu在I0系下的位姿
              traj_manager_->addPivotPointPlaneMeasurement(p_eigen, coeff_eigen, 
                                                          pivot_imu_pos, pivot_imu_rot,
                                                          frame_timestamp_buf_[opt_i] - 0.1 + s);
              
            }
          }
          LOG(INFO) << "addPivotPointPlaneMeasurement, t = " << tic_toc_.Toc() << " ms";

          // 添加 IMU 残差
          tic_toc_.Tic();
          for (int i = 1; i < estimator_config_.opt_window_size + 1; ++i) {
            int opt_i = int(estimator_config_.window_size - estimator_config_.opt_window_size + i);
            traj_manager_->addGyroscopeMeasurements(angular_velocity_buf_[opt_i], imu_timestamp_buf_[opt_i]);
            traj_manager_->addAccelerometerMeasurement(linear_acceleration_buf_[opt_i], imu_timestamp_buf_[opt_i]);  
          }
          LOG(INFO) << "addIMUMeasurements, t = " << tic_toc_.Toc() << " ms";
          LOG(INFO) << "in normal solve opt problem";
          tic_toc_.Tic();
          // 固定头4个控制点，避免pivot处的位姿改变（是否应该固定4个？）
          // traj_manager_->getTrajectory()->R3Spline()->LockContralPoint(0, 1);
          // traj_manager_->getTrajectory()->SO3Spline()->LockContralPoint(0, 1);
          bool is_convergence = false;
          traj_manager_->solveOptProblem(is_convergence);
          LOG(INFO) << "convergence type = " << is_convergence;
          LOG(INFO) << "solveOptProblem, t = " << tic_toc_.Toc() << " ms";
          traj_manager_->SetCalibParam();
          traj_manager_->ShowCalibParam();
          for (int i = 0; i < estimator_config_.opt_window_size + 1; ++i) {
            int opt_i = int(estimator_config_.window_size - estimator_config_.opt_window_size + i);
            Eigen::Vector3d pos, vel;
            Eigen::Quaterniond rot;
            if (!traj_manager_->evaluateLidarPoseAlignGravity(frame_timestamp_buf_[opt_i], pos, rot)) {
              LOG(ERROR) << "can't get lidar pose at index " << opt_i << " t = " << frame_timestamp_buf_[opt_i];
              continue;
            }
            if (!traj_manager_->evaluateImuVelocityAlignGravity(frame_timestamp_buf_[opt_i], vel)) {
              LOG(ERROR) << "can't get imu velocity at index " << opt_i << " t = " << frame_timestamp_buf_[opt_i];
              continue;
            }
            LOG(INFO) << "after opt pivot+" << i << " pos = " << pos.transpose() 
            << "  rot = " << rot.toRotationMatrix().eulerAngles(2, 1, 0).transpose()
            << "  vel = " << vel.transpose();
            LOG(INFO) << "print R3 contral point at t = " << frame_timestamp_buf_[opt_i] <<
            " typeid(vector3d) " << typeid(Eigen::Vector3d()).name();
            traj_manager_->getTrajectory()->R3Spline()->printContralPointWithTime(frame_timestamp_buf_[opt_i]);
            LOG(INFO) << "print So3 contral point at t = " << frame_timestamp_buf_[opt_i] <<
            " typeid(quaterniond) " << typeid(Eigen::Quaterniond()).name();
            traj_manager_->getTrajectory()->SO3Spline()->printContralPointWithTime(frame_timestamp_buf_[opt_i]);
          }

            if (is_convergence) break;

          }
          std::cout << "end opt" << std::endl << std::endl << std::endl;
          // 发送点云
          tic_toc_.Tic();
          SendCloudAndPose();
          LOG(INFO) << "SendCloudAndPose, t = " << tic_toc_.Toc() << " ms";

          // 滑窗
          tic_toc_.Tic();
          SlideWindow();

          // 获取最后的位姿，为预测位姿做准备
          GetLastPVQ();
          LOG(INFO) << "SlideWindow, t = " << tic_toc_.Toc() << " ms";
          LOG(INFO) << ">>>>>>> solving optimization <<<<<<<";

        } else {
        }
        break;
      }
      default: {
        break;
      }

    }
  }

  wi_trans_.setRotation(tf::Quaternion{Q_WI_.x(), Q_WI_.y(), Q_WI_.z(), Q_WI_.w()});
  wi_trans_.stamp_ = header.stamp;
  tf_broadcaster_est_.sendTransform(wi_trans_);

}

void Estimator::ProcessCompactData(const sensor_msgs::PointCloud2ConstPtr &compact_data,
                                   const std_msgs::Header &header) {
  /// 1. process compact data
  PointMapping::CompactDataHandler(compact_data);

  if (stage_flag_ == INITED) {

    if (estimator_config_.imu_factor) {
      //    // WARNING: or using direct date?
    } else {
      TransformAssociateToMap();
      DLOG(INFO) << ">>>>> transform original tobe <<<<<: " << transform_tobe_mapped_;
    }

  }

  if (stage_flag_ != INITED || !estimator_config_.imu_factor) {
    /// 2. process decoded data
    PointMapping::Process();
  } else {
//    for (int i = 0; i < laser_cloud_surf_last_->size(); ++i) {
//      PointT &p = laser_cloud_surf_last_->at(i);
//      p.intensity = p.intensity - int(p.intensity);
//    }
//
//    for (int i = 0; i < laser_cloud_corner_last_->size(); ++i) {
//      PointT &p = laser_cloud_corner_last_->at(i);
//      p.intensity = p.intensity - int(p.intensity);
//    }
//
//    laser_cloud_surf_stack_downsampled_->clear();
//    down_size_filter_surf_.setInputCloud(laser_cloud_surf_last_);
//    down_size_filter_surf_.filter(*laser_cloud_surf_stack_downsampled_);
//    size_t laser_cloud_surf_stack_ds_size = laser_cloud_surf_stack_downsampled_->points.size();
//
//    // down sample feature stack clouds
//    laser_cloud_corner_stack_downsampled_->clear();
//    down_size_filter_corner_.setInputCloud(laser_cloud_corner_last_);
//    down_size_filter_corner_.filter(*laser_cloud_corner_stack_downsampled_);
//    size_t laser_cloud_corner_stack_ds_size = laser_cloud_corner_stack_downsampled_->points.size();
  }

  DLOG(INFO) << "laser_cloud_surf_last_[" << header.stamp.toSec() << "]: "
            << laser_cloud_surf_last_->size();
  DLOG(INFO) << "laser_cloud_corner_last_[" << header.stamp.toSec() << "]: "
            << laser_cloud_corner_last_->size();

  DLOG(INFO) << endl << "transform_aft_mapped_[" << header.stamp.toSec() << "]: " << transform_aft_mapped_;
  DLOG(INFO) << "laser_cloud_surf_stack_downsampled_[" << header.stamp.toSec() << "]: "
            << laser_cloud_surf_stack_downsampled_->size();
  DLOG(INFO) << "laser_cloud_corner_stack_downsampled_[" << header.stamp.toSec() << "]: "
            << laser_cloud_corner_stack_downsampled_->size();

  Transform transform_to_init_ = transform_aft_mapped_;
  ProcessLaserOdom(transform_to_init_, header);

// NOTE: will be updated in PointMapping's OptimizeTransformTobeMapped
//  if (stage_flag_ == INITED && !estimator_config_.imu_factor) {
//    TransformUpdate();
//    DLOG(INFO) << ">>>>> transform sum <<<<<: " << transform_sum_;
//  }

}

bool Estimator::RunInitialization() {

}

#ifdef USE_CORNER
void Estimator::CalculateFeatures(const pcl::KdTreeFLANN<PointT>::Ptr &kdtree_surf_from_map,
                                  const PointCloudPtr &local_surf_points_filtered_ptr,
                                  const PointCloudPtr &surf_stack,
                                  const pcl::KdTreeFLANN<PointT>::Ptr &kdtree_corner_from_map,
                                  const PointCloudPtr &local_corner_points_filtered_ptr,
                                  const PointCloudPtr &corner_stack,
                                  const Transform &local_transform,
                                  vector<unique_ptr<Feature>> &features) {
#else
void Estimator::CalculateFeatures(const pcl::KdTreeFLANN<PointT>::Ptr &kdtree_surf_from_map,
                                  const PointCloudPtr &local_surf_points_filtered_ptr,
                                  const PointCloudPtr &surf_stack,
                                  const Transform &local_transform,
                                  vector<unique_ptr<Feature>> &features) {
#endif

  PointT point_sel, point_ori, point_proj, coeff1, coeff2;
  if (!estimator_config_.keep_features) {
    features.clear();
  }

  std::vector<int> point_search_idx(5, 0);
  std::vector<float> point_search_sq_dis(5, 0);
  Eigen::Matrix<float, 5, 3> mat_A0;
  Eigen::Matrix<float, 5, 1> mat_B0;
  Eigen::Vector3f mat_X0;
  Eigen::Matrix3f mat_A1;
  Eigen::Matrix<float, 1, 3> mat_D1;
  Eigen::Matrix3f mat_V1;

  mat_A0.setZero();
  mat_B0.setConstant(-1);
  mat_X0.setZero();

  mat_A1.setZero();
  mat_D1.setZero();
  mat_V1.setZero();

  PointCloud laser_cloud_ori;
  PointCloud coeff_sel;
  vector<float> scores;

  const PointCloudPtr &origin_surf_points = surf_stack;
  const Transform &transform_to_local = local_transform;
  size_t surf_points_size = origin_surf_points->points.size();

#ifdef USE_CORNER
  const PointCloudPtr &origin_corner_points = corner_stack;
  size_t corner_points_size = origin_corner_points->points.size();
#endif

//    DLOG(INFO) << "transform_to_local: " << transform_to_local;

  for (int i = 0; i < surf_points_size; i++) {
    point_ori = origin_surf_points->points[i];
    // if (!std::isfinite(point_ori.x) || !std::isfinite(point_ori.y) || !std::isfinite(point_ori.z)) continue;
    PointAssociateToMap(point_ori, point_sel, transform_to_local);

    int num_neighbors = 5;
    kdtree_surf_from_map->nearestKSearch(point_sel, num_neighbors, point_search_idx, point_search_sq_dis);

    if (point_search_sq_dis[num_neighbors - 1] < min_match_sq_dis_) {
      for (int j = 0; j < num_neighbors; j++) {
        mat_A0(j, 0) = local_surf_points_filtered_ptr->points[point_search_idx[j]].x;
        mat_A0(j, 1) = local_surf_points_filtered_ptr->points[point_search_idx[j]].y;
        mat_A0(j, 2) = local_surf_points_filtered_ptr->points[point_search_idx[j]].z;
      }
      mat_X0 = mat_A0.colPivHouseholderQr().solve(mat_B0);

      float pa = mat_X0(0, 0);
      float pb = mat_X0(1, 0);
      float pc = mat_X0(2, 0);
      float pd = 1;

      float ps = sqrt(pa * pa + pb * pb + pc * pc);
      pa /= ps;
      pb /= ps;
      pc /= ps;
      pd /= ps;

      // NOTE: plane as (x y z)*w+1 = 0

      bool planeValid = true;
      for (int j = 0; j < num_neighbors; j++) {
        if (fabs(pa * local_surf_points_filtered_ptr->points[point_search_idx[j]].x +
            pb * local_surf_points_filtered_ptr->points[point_search_idx[j]].y +
            pc * local_surf_points_filtered_ptr->points[point_search_idx[j]].z + pd) > min_plane_dis_) {
          planeValid = false;
          break;
        }
      }

      if (planeValid) {

        float pd2 = pa * point_sel.x + pb * point_sel.y + pc * point_sel.z + pd;

        float s = 1 - 0.9f * fabs(pd2) / sqrt(CalcPointDistance(point_sel));

        coeff1.x = s * pa;
        coeff1.y = s * pb;
        coeff1.z = s * pc;
        coeff1.intensity = s * pd;

        bool is_in_laser_fov = false;
        PointT transform_pos;
        PointT point_on_z_axis;

        point_on_z_axis.x = 0.0;
        point_on_z_axis.y = 0.0;
        point_on_z_axis.z = 10.0;
        PointAssociateToMap(point_on_z_axis, point_on_z_axis, transform_to_local);

        transform_pos.x = transform_to_local.pos.x();
        transform_pos.y = transform_to_local.pos.y();
        transform_pos.z = transform_to_local.pos.z();
        float squared_side1 = CalcSquaredDiff(transform_pos, point_sel);
        float squared_side2 = CalcSquaredDiff(point_on_z_axis, point_sel);

        float check1 = 100.0f + squared_side1 - squared_side2
            - 10.0f * sqrt(3.0f) * sqrt(squared_side1);

        float check2 = 100.0f + squared_side1 - squared_side2
            + 10.0f * sqrt(3.0f) * sqrt(squared_side1);

        if (check1 < 0 && check2 > 0) { /// within +-60 degree
          is_in_laser_fov = true;
        }

        if (s > 0.1 && is_in_laser_fov) {
          unique_ptr<PointPlaneFeature> feature = std::make_unique<PointPlaneFeature>();
          // feature->score = s;
          feature->score = point_ori.intensity - int(point_ori.intensity);
          feature->point = Eigen::Vector3d{point_ori.x, point_ori.y, point_ori.z};
          feature->coeffs = Eigen::Vector4d{coeff1.x, coeff1.y, coeff1.z, coeff1.intensity};
          features.push_back(std::move(feature));
        }
      }
    }
  }

#ifdef USE_CORNER
  //region Corner points
  for (int i = 0; i < corner_points_size; i++) {
    point_ori = origin_corner_points->points[i];
    PointAssociateToMap(point_ori, point_sel, transform_to_local);
    kdtree_corner_from_map->nearestKSearch(point_sel, 5, point_search_idx, point_search_sq_dis);

    if (point_search_sq_dis[4] < min_match_sq_dis_) {
      Eigen::Vector3f vc(0, 0, 0);

      for (int j = 0; j < 5; j++) {
        const PointT &point_sel_tmp = local_corner_points_filtered_ptr->points[point_search_idx[j]];
        vc.x() += point_sel_tmp.x;
        vc.y() += point_sel_tmp.y;
        vc.z() += point_sel_tmp.z;
      }
      vc /= 5.0;

      Eigen::Matrix3f mat_a;
      mat_a.setZero();

      for (int j = 0; j < 5; j++) {
        const PointT &point_sel_tmp = local_corner_points_filtered_ptr->points[point_search_idx[j]];
        Eigen::Vector3f a;
        a.x() = point_sel_tmp.x - vc.x();
        a.y() = point_sel_tmp.y - vc.y();
        a.z() = point_sel_tmp.z - vc.z();

        mat_a(0, 0) += a.x() * a.x();
        mat_a(1, 0) += a.x() * a.y();
        mat_a(2, 0) += a.x() * a.z();
        mat_a(1, 1) += a.y() * a.y();
        mat_a(2, 1) += a.y() * a.z();
        mat_a(2, 2) += a.z() * a.z();
      }
      mat_A1 = mat_a / 5.0;

      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> esolver(mat_A1);
      mat_D1 = esolver.eigenvalues().real();
      mat_V1 = esolver.eigenvectors().real();

      if (mat_D1(0, 2) > 3 * mat_D1(0, 1)) {

        float x0 = point_sel.x;
        float y0 = point_sel.y;
        float z0 = point_sel.z;
        float x1 = vc.x() + 0.1 * mat_V1(0, 2);
        float y1 = vc.y() + 0.1 * mat_V1(1, 2);
        float z1 = vc.z() + 0.1 * mat_V1(2, 2);
        float x2 = vc.x() - 0.1 * mat_V1(0, 2);
        float y2 = vc.y() - 0.1 * mat_V1(1, 2);
        float z2 = vc.z() - 0.1 * mat_V1(2, 2);

        Eigen::Vector3f X0(x0, y0, z0);
        Eigen::Vector3f X1(x1, y1, z1);
        Eigen::Vector3f X2(x2, y2, z2);

        Eigen::Vector3f a012_vec = (X0 - X1).cross(X0 - X2);

        Eigen::Vector3f normal_to_point = ((X1 - X2).cross(a012_vec)).normalized();

        Eigen::Vector3f normal_cross_point = (X1 - X2).cross(normal_to_point);

        float a012 = a012_vec.norm();

        float l12 = (X1 - X2).norm();

        float la = normal_to_point.x();
        float lb = normal_to_point.y();
        float lc = normal_to_point.z();

        float ld2 = a012 / l12;

        point_proj = point_sel;
        point_proj.x -= la * ld2;
        point_proj.y -= lb * ld2;
        point_proj.z -= lc * ld2;

        float ld_p1 = -(normal_to_point.x() * point_proj.x + normal_to_point.y() * point_proj.y
            + normal_to_point.z() * point_proj.z);
        float ld_p2 = -(normal_cross_point.x() * point_proj.x + normal_cross_point.y() * point_proj.y
            + normal_cross_point.z() * point_proj.z);

        float s = 1 - 0.9f * fabs(ld2);

        coeff1.x = s * la;
        coeff1.y = s * lb;
        coeff1.z = s * lc;
        coeff1.intensity = s * ld_p1;

        coeff2.x = s * normal_cross_point.x();
        coeff2.y = s * normal_cross_point.y();
        coeff2.z = s * normal_cross_point.z();
        coeff2.intensity = s * ld_p2;

        bool is_in_laser_fov = false;
        PointT transform_pos;
        transform_pos.x = transform_tobe_mapped_.pos.x();
        transform_pos.y = transform_tobe_mapped_.pos.y();
        transform_pos.z = transform_tobe_mapped_.pos.z();
        float squared_side1 = CalcSquaredDiff(transform_pos, point_sel);
        float squared_side2 = CalcSquaredDiff(point_on_z_axis_, point_sel);

        float check1 = 100.0f + squared_side1 - squared_side2
            - 10.0f * sqrt(3.0f) * sqrt(squared_side1);

        float check2 = 100.0f + squared_side1 - squared_side2
            + 10.0f * sqrt(3.0f) * sqrt(squared_side1);

        if (check1 < 0 && check2 > 0) { /// within +-60 degree
          is_in_laser_fov = true;
        }

        if (s > 0.1 && is_in_laser_fov) {
          unique_ptr<PointPlaneFeature> feature1 = std::make_unique<PointPlaneFeature>();
          feature1->score = s * 0.5;
          feature1->point = Eigen::Vector3d{point_ori.x, point_ori.y, point_ori.z};
          feature1->coeffs = Eigen::Vector4d{coeff1.x, coeff1.y, coeff1.z, coeff1.intensity} * 0.5;
          features.push_back(std::move(feature1));

          unique_ptr<PointPlaneFeature> feature2 = std::make_unique<PointPlaneFeature>();
          feature2->score = s * 0.5;
          feature2->point = Eigen::Vector3d{point_ori.x, point_ori.y, point_ori.z};
          feature2->coeffs = Eigen::Vector4d{coeff2.x, coeff2.y, coeff2.z, coeff2.intensity} * 0.5;
          features.push_back(std::move(feature2));
        }
      }
    }
  }
  //endregion
#endif
}

#ifdef USE_CORNER
void Estimator::CalculateLaserOdom(const pcl::KdTreeFLANN<PointT>::Ptr &kdtree_surf_from_map,
                                   const PointCloudPtr &local_surf_points_filtered_ptr,
                                   const PointCloudPtr &surf_stack,
                                   const pcl::KdTreeFLANN<PointT>::Ptr &kdtree_corner_from_map,
                                   const PointCloudPtr &local_corner_points_filtered_ptr,
                                   const PointCloudPtr &corner_stack,
                                   Transform &local_transform,
                                   vector<unique_ptr<Feature>> &features) {
#else
void Estimator::CalculateLaserOdom(const pcl::KdTreeFLANN<PointT>::Ptr &kdtree_surf_from_map,
                                   const PointCloudPtr &local_surf_points_filtered_ptr,
                                   const PointCloudPtr &surf_stack,
                                   Transform &local_transform,
                                   vector<unique_ptr<Feature>> &features) {
#endif

  bool is_degenerate = false;
  for (size_t iter_count = 0; iter_count < num_max_iterations_; ++iter_count) {

#ifdef USE_CORNER
    CalculateFeatures(kdtree_surf_from_map, local_surf_points_filtered_ptr, surf_stack,
                      kdtree_corner_from_map, local_corner_points_filtered_ptr, corner_stack,
                      local_transform, features);
#else
    CalculateFeatures(kdtree_surf_from_map, local_surf_points_filtered_ptr, surf_stack,
                      local_transform, features);
#endif

    size_t laser_cloud_sel_size = features.size();
    Eigen::Matrix<float, Eigen::Dynamic, 6> mat_A(laser_cloud_sel_size, 6);
    Eigen::Matrix<float, 6, Eigen::Dynamic> mat_At(6, laser_cloud_sel_size);
    Eigen::Matrix<float, 6, 6> matAtA;
    Eigen::VectorXf mat_B(laser_cloud_sel_size);
    Eigen::VectorXf mat_AtB;
    Eigen::VectorXf mat_X;
    Eigen::Matrix<float, 6, 6> matP;

    PointT point_sel, point_ori, coeff;

    SO3 R_SO3(local_transform.rot); /// SO3

    for (int i = 0; i < laser_cloud_sel_size; i++) {
      PointPlaneFeature feature_i;
      features[i]->GetFeature(&feature_i);
      point_ori.x = feature_i.point.x();
      point_ori.y = feature_i.point.y();
      point_ori.z = feature_i.point.z();
      coeff.x = feature_i.coeffs.x();
      coeff.y = feature_i.coeffs.y();
      coeff.z = feature_i.coeffs.z();
      coeff.intensity = feature_i.coeffs.w();

      Eigen::Vector3f p(point_ori.x, point_ori.y, point_ori.z);
      Eigen::Vector3f w(coeff.x, coeff.y, coeff.z);

//      Eigen::Vector3f J_r = w.transpose() * RotationVectorJacobian(R_SO3, p);
      Eigen::Vector3f J_r = -w.transpose() * (local_transform.rot * SkewSymmetric(p));
      Eigen::Vector3f J_t = w.transpose();

      float d2 = w.transpose() * (local_transform.rot * p + local_transform.pos) + coeff.intensity;

      mat_A(i, 0) = J_r.x();
      mat_A(i, 1) = J_r.y();
      mat_A(i, 2) = J_r.z();
      mat_A(i, 3) = J_t.x();
      mat_A(i, 4) = J_t.y();
      mat_A(i, 5) = J_t.z();
      mat_B(i, 0) = -d2;
    }

    mat_At = mat_A.transpose();
    matAtA = mat_At * mat_A;
    mat_AtB = mat_At * mat_B;
    mat_X = matAtA.colPivHouseholderQr().solve(mat_AtB);

    if (iter_count == 0) {
      Eigen::Matrix<float, 1, 6> mat_E;
      Eigen::Matrix<float, 6, 6> mat_V;
      Eigen::Matrix<float, 6, 6> mat_V2;

      Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 6, 6>> esolver(matAtA);
      mat_E = esolver.eigenvalues().real();
      mat_V = esolver.eigenvectors().real();

      mat_V2 = mat_V;

      is_degenerate = false;
      float eignThre[6] = {100, 100, 100, 100, 100, 100};
      for (int i = 0; i < 6; ++i) {
        if (mat_E(0, i) < eignThre[i]) {
          for (int j = 0; j < 6; ++j) {
            mat_V2(i, j) = 0;
          }
          is_degenerate = true;
          DLOG(WARNING) << "degenerate case";
          DLOG(INFO) << mat_E;
        } else {
          break;
        }
      }
      matP = mat_V2 * mat_V.inverse();
    }

    if (is_degenerate) {
      Eigen::Matrix<float, 6, 1> matX2(mat_X);
      mat_X = matP * matX2;
    }

    local_transform.pos.x() += mat_X(3, 0);
    local_transform.pos.y() += mat_X(4, 0);
    local_transform.pos.z() += mat_X(5, 0);

    local_transform.rot = local_transform.rot * DeltaQ(Eigen::Vector3f(mat_X(0, 0), mat_X(1, 0), mat_X(2, 0)));

    if (!isfinite(local_transform.pos.x())) local_transform.pos.x() = 0.0;
    if (!isfinite(local_transform.pos.y())) local_transform.pos.y() = 0.0;
    if (!isfinite(local_transform.pos.z())) local_transform.pos.z() = 0.0;

    float delta_r = RadToDeg(R_SO3.unit_quaternion().angularDistance(local_transform.rot));
    float delta_t = sqrt(pow(mat_X(3, 0) * 100, 2) + pow(mat_X(4, 0) * 100, 2) + pow(mat_X(5, 0) * 100, 2));

    if (delta_r < delta_r_abort_ && delta_t < delta_t_abort_) {
      DLOG(INFO) << "CalculateLaserOdom iter_count: " << iter_count;
      break;
    }
  }
}

void Estimator::BuildLocalMapSplineInit(vector<FeaturePerFrame> &feature_frames) {

}

void Estimator::BuildLocalMapSplineNormal(vector<FeaturePerFrame> &feature_frames, 
                                          Eigen::Vector3d &pos_imu_pivot, 
                                          Eigen::Quaterniond &rot_imu_pivot, bool first_opt) {
  feature_frames.clear();

  TicToc t_build_map;

  local_surf_points_ptr_.reset();
  local_surf_points_ptr_ = boost::make_shared<PointCloud>(PointCloud());

  local_surf_points_filtered_ptr_.reset();
  local_surf_points_filtered_ptr_ = boost::make_shared<PointCloud>(PointCloud());

#ifdef USE_CORNER
  local_corner_points_ptr_.reset();
  local_corner_points_ptr_ = boost::make_shared<PointCloud>(PointCloud());

  local_corner_points_filtered_ptr_.reset();
  local_corner_points_filtered_ptr_ = boost::make_shared<PointCloud>(PointCloud());
#endif

  vector<Transform> local_transforms;
  int pivot_idx = estimator_config_.window_size - estimator_config_.opt_window_size;

  {
    for (int i = 0; i < estimator_config_.window_size + 1; ++i) {
      if (i < pivot_idx) {
        Transform local_transform(Transform::Identity());
        local_transforms.push_back(local_transform);
        continue;
      }
      Eigen::Affine3f transform_pivot_i;
      if (!first_opt) {
        Eigen::Vector3d pos_li, pos_lp;
        Eigen::Quaterniond rot_li, rot_lp;
        // double time_pivot = frame_timestamp_buf_[pivot_idx];
        double time_i = frame_timestamp_buf_[i];
        traj_manager_->evaluateLidarPose(time_i, pos_li, rot_li);
        Twist<double> transform_li = Twist<double>(rot_li, pos_li);
        // 计算T_Li^I0
        rot_lp = rot_imu_pivot * traj_manager_->calib_param_manager->q_LtoI;
        pos_lp = rot_imu_pivot * traj_manager_->calib_param_manager->p_LinI + pos_imu_pivot;
        Twist<double> transform_lp = Twist<double>(rot_lp, pos_lp);
        transform_pivot_i = (transform_lp.inverse() * transform_li).cast<float>().transform();

        Transform local_transform = transform_pivot_i;
        local_transforms.push_back(local_transform);

      } else if (i == estimator_config_.window_size) {
        Eigen::Vector3d Ps_i;
        Eigen::Matrix3d Rs_i;
        Eigen::Vector3d p_LinI, p_pivot;
        Eigen::Quaterniond q_LtoI, q_pivot;
        traj_manager_->getExtrinsic(p_LinI, q_LtoI);
        Quaterniond rot_li(Rs_i * q_LtoI);
        Eigen::Vector3d pos_li = Ps_i + Rs_i * p_LinI;

        Twist<double> transform_li = Twist<double>(rot_li, pos_li);

        traj_manager_->evaluateLidarPoseAlignGravity(frame_timestamp_buf_[pivot_idx], p_pivot, q_pivot);
        Twist<double> transform_pivot = Twist<double>(q_pivot, p_pivot);
        transform_pivot_i = (transform_pivot.inverse() * transform_li).cast<float>().transform();
        LOG(INFO) << "in build map pos pivot = " << p_pivot.transpose();
        LOG(INFO) << "in build map rot pivot = " << q_pivot.toRotationMatrix().eulerAngles(2, 1, 0).transpose();
        Transform local_transform = transform_pivot_i;
        local_transforms.push_back(local_transform);
      } else {
        Eigen::Vector3d pos_pivot_i;
        Eigen::Quaterniond rot_pivot_i;
        double time_pivot = frame_timestamp_buf_[pivot_idx];
        double time_i = frame_timestamp_buf_[i];
        traj_manager_->evaluateLidarRelativePose(time_pivot, time_i, rot_pivot_i, pos_pivot_i);
        transform_pivot_i.linear() = rot_pivot_i.toRotationMatrix().template cast<float>();
        transform_pivot_i.translation() = pos_pivot_i.template cast<float>();

        Transform local_transform = transform_pivot_i;
        local_transforms.push_back(local_transform);
      }

      PointCloud transformed_cloud_surf, transformed_cloud_corner;
      //region fix the map
#ifdef FIX_MAP
      Eigen::Vector3d Ps_linearized_i = Ps_linearized_[i];
      Eigen::Matrix3d Rs_linearized_i = Rs_linearized_[i];

      Quaterniond rot_linearized_i(Rs_linearized_i * transform_lb.rot.inverse());
      Eigen::Vector3d pos_linearized_i = Ps_linearized_i - rot_linearized_i * transform_lb.pos;

      Twist<double> transform_linearized_i = Twist<double>(rot_linearized_i, pos_linearized_i);
      Eigen::Affine3f transform_linearized_pivot_i =
          (transform_linearized_pivot.inverse() * transform_linearized_i).cast<float>().transform();
      DLOG(INFO) << "transform_li: " << transform_li;
      DLOG(INFO) << "transform_linearized_i: " << transform_linearized_i;
#endif
      //endregion


      // NOTE: exclude the latest one
      if (i != estimator_config_.window_size) {
        if (i == pivot_idx) {
          *local_surf_points_ptr_ += *(surf_stack_[i]);
//          down_size_filter_surf_.setInputCloud(local_surf_points_ptr_);
//          down_size_filter_surf_.filter(transformed_cloud_surf);
//          *local_surf_points_ptr_ = transformed_cloud_surf;
#ifdef USE_CORNER
          *local_corner_points_ptr_ += *(corner_stack_[i]);
#endif
          continue;
        }
        //region fix the map
#ifdef FIX_MAP
        pcl::transformPointCloud(*(surf_stack_[i]), transformed_cloud_surf, transform_linearized_pivot_i);
#ifdef USE_CORNER
        pcl::transformPointCloud(*(corner_stack_[i]), transformed_cloud_corner, transform_linearized_pivot_i);
#endif
#else
        pcl::transformPointCloud(*(surf_stack_[i]), transformed_cloud_surf, transform_pivot_i);
        LOG(INFO) << "in build map transform_pivot_" << i << " pos = " << transform_pivot_i.translation().transpose();
        LOG(INFO) << "in build map transform_pivot_" << i << " rot = " << transform_pivot_i.rotation().eulerAngles(2, 1, 0).transpose();
#ifdef USE_CORNER
        pcl::transformPointCloud(*(corner_stack_[i]), transformed_cloud_corner, transform_pivot_i);
#endif
#endif
        //endregion
        for (int p_idx = 0; p_idx < transformed_cloud_surf.size(); ++p_idx) {
          transformed_cloud_surf[p_idx].intensity = i;
        }
        *local_surf_points_ptr_ += transformed_cloud_surf;
#ifdef USE_CORNER
        for (int p_idx = 0; p_idx < transformed_cloud_corner.size(); ++p_idx) {
          transformed_cloud_corner[p_idx].intensity = i;
        }
        *local_corner_points_ptr_ += transformed_cloud_corner;
#endif
      }
    }

    DLOG(INFO) << "local_surf_points_ptr_->size() bef: " << local_surf_points_ptr_->size();
    down_size_filter_surf_.setInputCloud(local_surf_points_ptr_);
    down_size_filter_surf_.filter(*local_surf_points_filtered_ptr_);
    DLOG(INFO) << "local_surf_points_ptr_->size() aft: " << local_surf_points_filtered_ptr_->size();

#ifdef USE_CORNER
    DLOG(INFO) << "local_corner_points_ptr_->size() bef: " << local_corner_points_ptr_->size();
    down_size_filter_corner_.setInputCloud(local_corner_points_ptr_);
    down_size_filter_corner_.filter(*local_corner_points_filtered_ptr_);
    DLOG(INFO) << "local_corner_points_ptr_->size() aft: " << local_corner_points_filtered_ptr_->size();
#endif

  }

  ROS_DEBUG_STREAM("t_build_map cost: " << t_build_map.Toc() << " ms");
  DLOG(INFO) << "t_build_map cost: " << t_build_map.Toc() << " ms";

  pcl::KdTreeFLANN<PointT>::Ptr kdtree_surf_from_map(new pcl::KdTreeFLANN<PointT>());
  kdtree_surf_from_map->setInputCloud(local_surf_points_filtered_ptr_);

#ifdef USE_CORNER
  pcl::KdTreeFLANN<PointT>::Ptr kdtree_corner_from_map(new pcl::KdTreeFLANN<PointT>());
  kdtree_corner_from_map->setInputCloud(local_corner_points_filtered_ptr_);
#endif

  for (int idx = 0; idx < estimator_config_.window_size + 1; ++idx) {

    FeaturePerFrame feature_per_frame;
    vector<unique_ptr<Feature>> features;
//    vector<unique_ptr<Feature>> &features = feature_per_frame.features;

    TicToc t_features;

    if (idx > pivot_idx) {
      if (!first_opt || idx != estimator_config_.window_size || !estimator_config_.imu_factor) {
      // if (1) {
#ifdef USE_CORNER
        CalculateFeatures(kdtree_surf_from_map, local_surf_points_filtered_ptr_, surf_stack_[idx],
                          kdtree_corner_from_map, local_corner_points_filtered_ptr_, corner_stack_[idx],
                          local_transforms[idx], features);
#else
        CalculateFeatures(kdtree_surf_from_map, local_surf_points_filtered_ptr_, surf_stack_[idx],
                          local_transforms[idx], features);
#endif
      } else {
#ifdef USE_CORNER
        CalculateLaserOdom(kdtree_surf_from_map, local_surf_points_filtered_ptr_, surf_stack_[idx],
                            kdtree_corner_from_map, local_corner_points_filtered_ptr_, corner_stack_[idx],
                            local_transforms[idx], features);
#else
        CalculateLaserOdom(kdtree_surf_from_map, local_surf_points_filtered_ptr_, surf_stack_[idx],
                            local_transforms[idx], features);
#endif
      }
    } else {
      // NOTE: empty features
    }

    feature_per_frame.id = idx;
//    feature_per_frame.features = std::move(features);
    feature_per_frame.features.assign(make_move_iterator(features.begin()), make_move_iterator(features.end()));
    feature_frames.push_back(std::move(feature_per_frame));

    ROS_DEBUG_STREAM("feature cost: " << t_features.Toc() << " ms");
  }
}

void Estimator::BuildLocalMap(vector<FeaturePerFrame> &feature_frames) {

}

void Estimator::SolveOptimization() {

}

void Estimator::SlideWindow() { // NOTE: this function is only for the states and the local map

}

void Estimator::ProcessEstimation() {

  while (true) {
    PairMeasurements measurements;
    std::unique_lock<std::mutex> buf_lk(buf_mutex_);
    con_.wait(buf_lk, [&] {
      return (measurements = GetMeasurements()).size() != 0;
    });
    buf_lk.unlock();

//    DLOG(INFO) << "measurement obtained: " << measurements.size() << ", first imu data size: "
//              << measurements.front().first.size();
    
    // 由于第一帧点云对应的 IMU 数据一般来说是多于 0.1s的，因此为方便起见，直接不要第一帧数据了。
    if (!first_imu_) {
      first_imu_ = true;
      continue;
    }

    thread_mutex_.lock();

    linear_acceleration_buf_.push(vector<Vector3d>());
    angular_velocity_buf_.push(vector<Vector3d>());
    imu_timestamp_buf_.push(vector<double>());

    for (auto &measurement : measurements) {
      ROS_DEBUG_STREAM("measurements ratio: 1:" << measurement.first.size());
      CompactDataConstPtr compact_data_msg = measurement.second;
      double ax = 0, ay = 0, az = 0, rx = 0, ry = 0, rz = 0;
      TicToc tic_toc_imu;
      tic_toc_imu.Tic();
      for (auto &imu_msg : measurement.first) {
        double imu_time = imu_msg->header.stamp.toSec();
        double laser_odom_time = compact_data_msg->header.stamp.toSec() + mm_config_.msg_time_delay;
        if (imu_time <= laser_odom_time) {

          if (curr_time_ < 0) {
            curr_time_ = imu_time;
          }

          double dt = imu_time - curr_time_;
          ROS_ASSERT(dt >= 0);
          curr_time_ = imu_time;
          ax = imu_msg->linear_acceleration.x;
          ay = imu_msg->linear_acceleration.y;
          az = imu_msg->linear_acceleration.z;
          rx = imu_msg->angular_velocity.x;
          ry = imu_msg->angular_velocity.y;
          rz = imu_msg->angular_velocity.z;
          // 保存imu原始数据
          linear_acceleration_buf_.last().push_back(Vector3d(ax, ay, az));
          angular_velocity_buf_.last().push_back(Vector3d(rx, ry, rz));
          imu_timestamp_buf_.last().push_back(imu_time);

        } else {

          // NOTE: interpolate imu measurement
          double dt_1 = laser_odom_time - curr_time_;
          double dt_2 = imu_time - laser_odom_time;
          curr_time_ = laser_odom_time;
          ROS_ASSERT(dt_1 >= 0);
          ROS_ASSERT(dt_2 >= 0);
          ROS_ASSERT(dt_1 + dt_2 > 0);
          double w1 = dt_2 / (dt_1 + dt_2);
          double w2 = dt_1 / (dt_1 + dt_2);
          ax = w1 * ax + w2 * imu_msg->linear_acceleration.x;
          ay = w1 * ay + w2 * imu_msg->linear_acceleration.y;
          az = w1 * az + w2 * imu_msg->linear_acceleration.z;
          rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
          ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
          rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
          // 保存imu原始数据
          linear_acceleration_buf_.last().push_back(Vector3d(ax, ay, az));
          angular_velocity_buf_.last().push_back(Vector3d(rx, ry, rz));
          imu_timestamp_buf_.last().push_back(laser_odom_time);
        }
      }

      DLOG(INFO) << "per imu time: " << tic_toc_imu.Toc() / measurement.first.size() << " ms";

      ROS_DEBUG("processing laser data with stamp %f \n", compact_data_msg->header.stamp.toSec());

      TicToc t_s;

      this->ProcessCompactData(compact_data_msg, compact_data_msg->header);

      double whole_t = t_s.Toc();
    }


    thread_mutex_.unlock();
  }

}

bool Estimator::SendCloudAndPose() {
  int pivot_idx = estimator_config_.window_size - estimator_config_.opt_window_size;
  int send_idx;
  // // 发送 pivot + 1 处的点云
  // PublishCloudMsg(pub_local_surf_points_,
  //                     *surf_stack_[send_idx],
  //                     Headers_[send_idx].stamp,
  //                     "laser_send");

  // PublishCloudMsg(pub_local_corner_points_,
  //                 *corner_stack_[send_idx],
  //                 Headers_[send_idx].stamp,
  //                 "laser_send");

  // PublishCloudMsg(pub_local_full_points_,
  //                 *full_stack_[send_idx],
  //                 Headers_[send_idx].stamp,
  //                 "laser_send");

  // PublishCloudMsg(pub_map_surf_points_,
  //                 *local_surf_points_filtered_ptr_,
  //                 Headers_[send_idx].stamp,
  //                 "laser_local");

  // 发送 pivot + 1 处的点云在水平系下的位姿
  // local_odom_.header.stamp = Headers_[send_idx].stamp;
  local_odom_.header.seq += 1;
  Eigen::Vector3d send_pos;
  Eigen::Quaterniond send_rot;
  double send_time = frame_timestamp_buf_[send_idx];
  traj_manager_->evaluateLidarPoseAlignGravity(send_time, send_pos, send_rot);
  local_odom_.pose.pose.orientation.x = send_rot.x();
  local_odom_.pose.pose.orientation.y = send_rot.y();
  local_odom_.pose.pose.orientation.z = send_rot.z();
  local_odom_.pose.pose.orientation.w = send_rot.w();
  local_odom_.pose.pose.position.x = send_pos.x();
  local_odom_.pose.pose.position.y = send_pos.y();
  local_odom_.pose.pose.position.z = send_pos.z();
  pub_local_odom_.publish(local_odom_);
  LOG(INFO) << "in SendCloudAndPose:";
  LOG(INFO) << "pivot+1: pos = " << send_pos.transpose() 
  << " rot = [x y z w]" << send_rot.x() << " " << send_rot.y() << " " << send_rot.z() << " " << send_rot.w();

  // 发送局部地图的tf以及发送到mapping处的tf，便于直观观察点云的配准情况。
  Eigen::Vector3d pos_pivot;
  Eigen::Quaterniond rot_pivot;
  traj_manager_->evaluateLidarPoseAlignGravity(frame_timestamp_buf_[pivot_idx], pos_pivot, rot_pivot);
  laser_local_trans_.setOrigin(tf::Vector3{pos_pivot.x(), pos_pivot.y(), pos_pivot.z()});
  laser_local_trans_.setRotation(tf::Quaternion{rot_pivot.x(), rot_pivot.y(), rot_pivot.z(), rot_pivot.w()});
  // laser_local_trans_.stamp_ = Headers_[send_idx].stamp;
  tf_broadcaster_est_.sendTransform(laser_local_trans_);

  laser_send_trans_.setOrigin(tf::Vector3{send_pos.x(), send_pos.y(), send_pos.z()});
  laser_send_trans_.setRotation(tf::Quaternion{send_rot.x(), send_rot.y(), send_rot.z(), send_rot.w()});
  // laser_send_trans_.stamp_ = Headers_[send_idx].stamp;
  tf_broadcaster_est_.sendTransform(laser_send_trans_);

  // 发送最后一帧点云，便于对比观察
  // PublishCloudMsg(pub_predict_surf_points_, *(surf_stack_.last()), Headers_.last().stamp, "laser_predict");
  // PublishCloudMsg(pub_predict_full_points_, *(full_stack_.last()), Headers_.last().stamp, "laser_predict");

  // PublishCloudMsg(pub_predict_corrected_full_points_,
  //                 *(full_stack_.last()),
  //                 Headers_.last().stamp,
  //                 "laser_predict");

#ifdef USE_CORNER
  PublishCloudMsg(pub_predict_corner_points_, *(corner_stack_.last()), Headers_.last().stamp, "laser_predict");
#endif
  
  // 发送最后一帧点云在水平系下的位姿
  // laser_odom_.header.stamp = Headers_.last().stamp;
  laser_odom_.header.seq += 1;
  Eigen::Vector3d predict_pos;
  Eigen::Quaterniond predict_rot;
  double predict_time = frame_timestamp_buf_.last();
  traj_manager_->evaluateLidarPoseAlignGravity(predict_time, predict_pos, predict_rot);
  laser_odom_.pose.pose.orientation.x = predict_rot.x();
  laser_odom_.pose.pose.orientation.y = predict_rot.y();
  laser_odom_.pose.pose.orientation.z = predict_rot.z();
  laser_odom_.pose.pose.orientation.w = predict_rot.w();
  laser_odom_.pose.pose.position.x = predict_pos.x();
  laser_odom_.pose.pose.position.y = predict_pos.y();
  laser_odom_.pose.pose.position.z = predict_pos.z();
  pub_laser_odom_.publish(laser_odom_);

  // 发送最后一帧点云的 tf
  Quaterniond rot_predict;
  Eigen::Vector3d pos_predict;
  double pivot_time = frame_timestamp_buf_[pivot_idx];
  traj_manager_->evaluateLidarRelativePose(pivot_time, predict_time, rot_predict, pos_predict);

  laser_predict_trans_.setOrigin(tf::Vector3{pos_predict.x(), pos_predict.y(), pos_predict.z()});
  laser_predict_trans_.setRotation(tf::Quaternion{rot_predict.x(), rot_predict.y(), rot_predict.z(),
                                                  rot_predict.w()});
  // laser_predict_trans_.stamp_ = Headers_.last().stamp;
  tf_broadcaster_est_.sendTransform(laser_predict_trans_);
  return true;
}

bool Estimator::SplineInitialization() {
  // TODO: 根据点云以及 IMU 时间申请控制点
  TicToc tic_all;
  tic_all.Tic();
  tic_toc_.Tic();
  int pivot_idx = estimator_config_.window_size - estimator_config_.opt_window_size;
  double start_time = frame_timestamp_buf_[pivot_idx];
  double end_time_lidar = frame_timestamp_buf_.last();
  double end_time_imu = imu_timestamp_buf_.last().back();
  LOG(INFO) << "in SplineInitialization t_last_lidar - t_last_imu = " << end_time_lidar - end_time_imu;
  double end_time = end_time_lidar > end_time_imu ? end_time_lidar : end_time_imu;
  traj_manager_ = std::make_shared<TrajectoryManager>(start_time, end_time, knot_distance_);

  // 初始化估计器
  traj_manager_->trajInit();

  // 添加初始位置的四元数约束
  traj_manager_->addInitQuaternionMeasurement();
  LOG(INFO) << "in initialization init traj_manager, t = " << tic_toc_.Toc() << " ms";

  // TODO: 添加测量数据残差
  tic_toc_.Tic();
  vector<FeaturePerFrame> feature_frames;
  BuildLocalMapSplineInit(feature_frames);
  LOG(INFO) << "in initialization BuildLocalMapSplineInit, t = " << tic_toc_.Toc() << " ms";
  // 根据找到的匹配对，添加残差
  tic_toc_.Tic();

  // 计算pivot处imu在I0系下的位姿
  Eigen::Vector3d pivot_imu_pos;
  Eigen::Quaterniond pivot_imu_rot;
  if (!traj_manager_->evaluateImuPose(frame_timestamp_buf_[pivot_idx], pivot_imu_pos, pivot_imu_rot)) {
    LOG(ERROR) << "could not get pivot pose at I0 with t= " << frame_timestamp_buf_[pivot_idx];
  }
  for (int i = 1; i < estimator_config_.opt_window_size + 1; ++i) {
    int opt_i = int(estimator_config_.window_size - estimator_config_.opt_window_size + i);
    int pivot_idx = int(estimator_config_.window_size - estimator_config_.opt_window_size);

    FeaturePerFrame &feature_per_frame = feature_frames[opt_i];
    LOG_ASSERT(opt_i == feature_per_frame.id);

    vector<unique_ptr<Feature>> &features = feature_per_frame.features;

    DLOG(INFO) << "features.size(): " << features.size();

    for (int j = 0; j < features.size(); ++j) {
      PointPlaneFeature feature_j;
      features[j]->GetFeature(&feature_j);

      const double &s = feature_j.score;

      Eigen::Vector3d &p_eigen = feature_j.point;
      Eigen::Vector4d &coeff_eigen = feature_j.coeffs;
      // traj_manager_->addPivotPointPlaneMeasurement(p_eigen, coeff_eigen, 
      //                                             frame_timestamp_buf_[pivot_idx],
      //                                             frame_timestamp_buf_[opt_i]);

      // 给出pivot处imu在I0系下的位姿
      traj_manager_->addPivotPointPlaneMeasurement(p_eigen, coeff_eigen, 
                                                  pivot_imu_pos, pivot_imu_rot,
                                                  frame_timestamp_buf_[opt_i]);
      
    }
  }
  LOG(INFO) << "in initialization addPivotPointPlaneMeasurement, t = " << tic_toc_.Toc() << " ms";

  // 添加 IMU 残差
  tic_toc_.Tic();
  for (int i = 1; i < estimator_config_.opt_window_size + 1; ++i) {
    int opt_i = int(estimator_config_.window_size - estimator_config_.opt_window_size + i);
    traj_manager_->addGyroscopeMeasurements(angular_velocity_buf_[opt_i], imu_timestamp_buf_[opt_i]);
    traj_manager_->addAccelerometerMeasurement(linear_acceleration_buf_[opt_i], imu_timestamp_buf_[opt_i]);  
  }
  LOG(INFO) << "in initialization addIMUMeasurements, t = " << tic_toc_.Toc() << " ms";
  tic_toc_.Tic();
  bool is_convergence;
  traj_manager_->solveOptProblem(is_convergence);
  LOG(INFO) << "in initialization solveOptProblem, t = " << tic_toc_.Toc() << " ms";
  tic_toc_.Tic();
  traj_manager_->SetCalibParam();
  LOG(INFO) << "in initialization SetCalibParam, t = " << tic_toc_.Toc() << " ms";
  traj_manager_->ShowCalibParam();

  // 发送点云
  tic_toc_.Tic();
  SendCloudAndPose();
  LOG(INFO) << "in initialization SendCloudAndPose, t = " << tic_toc_.Toc() << " ms";

  // 滑窗
  tic_toc_.Tic();
  SlideWindow();
  // 获取最后的位姿，为预测位姿做准备
  GetLastPVQ();
  LOG(INFO) << "in initialization SlideWindow, t = " << tic_toc_.Toc()<< " ms";

  LOG(INFO) << "in initialization all time, t = " << tic_all.Toc() << " ms";
  return true;
}

bool Estimator::SetNewContralPoint() {
  tic_toc_.Tic();
  traj_manager_->trajInitForNewContralPoint();
  traj_manager_->addGyroscopeMeasurements(angular_velocity_buf_.last(), imu_timestamp_buf_.last());
  traj_manager_->addAccelerometerMeasurement(linear_acceleration_buf_.last(), imu_timestamp_buf_.last());
  bool is_convergence;
  traj_manager_->solveOptProblem(is_convergence);
  traj_manager_->ShowCalibParam();
  if (stage_flag_ == INITED) {
    // predict_spline_odom_.header.stamp = Headers_.last().stamp;
    predict_spline_odom_.header.seq += 1;
    Eigen::Quaterniond quat;
    Eigen::Vector3d pos;
    LOG(INFO) << "in SetNewContralPoint before evaluateLidarPoseAlignGravity";
    traj_manager_->evaluateLidarPoseAlignGravity(frame_timestamp_buf_.last(), pos, quat);
    LOG(INFO) << "in SetNewContralPoint after evaluateLidarPoseAlignGravity";
    predict_spline_odom_.pose.pose.orientation.x = quat.x();
    predict_spline_odom_.pose.pose.orientation.y = quat.y();
    predict_spline_odom_.pose.pose.orientation.z = quat.z();
    predict_spline_odom_.pose.pose.orientation.w = quat.w();
    predict_spline_odom_.pose.pose.position.x = pos.x();
    predict_spline_odom_.pose.pose.position.y = pos.y();
    predict_spline_odom_.pose.pose.position.z = pos.z();

    pub_predict_spline_odom_.publish(predict_spline_odom_);
  }
  LOG(INFO) << "in SetNewContralPoint t = " << tic_toc_.Toc() << " ms";
  return true;
}

bool Estimator::GetLastPVQ() {
  int pivot_idx = estimator_config_.window_size - estimator_config_.opt_window_size;
  LOG(INFO) << "\033[32m last frame time = " << frame_timestamp_buf_.last() << "\033[0m";
  // for (int i = estimator_config_.window_size; i > pivot_idx; --i) {
    // if (!traj_manager_->evaluateImuVelocityAlignGravity(frame_timestamp_buf_.last(), Vs_.last())) {
    //   LOG(ERROR) << "could not get v at t = " << frame_timestamp_buf_.last();
    //   return false;
    // }
    
    // if (Vs_.last().norm() > 1.0) {
    //   LOG(INFO) << "\033[32m Vs last norm = "<< Vs_.last().norm() << "> 1 \033[0m";
    //   // Vs_.last().setZero();
    // }
  // }
  Eigen::Quaterniond qlast;
  // if (!traj_manager_->evaluateImuPoseAlignGravity(frame_timestamp_buf_.last(), Ps_.last(), qlast)) return false;
  // Rs_.last() = qlast.toRotationMatrix();
  // Bas_.last() = traj_manager_->calib_param_manager->acce_bias;
  // Bgs_.last() = traj_manager_->calib_param_manager->gyro_bias;
  LOG(INFO) << "has get last P V Q";
  return true;
}

} // namespace lio